//
// Created by DELL on 2023/11/27.
//

#include "bsp_can.h"

#include "driver_communicate.h"
#include "driver_chassis.h"
#include "task_lostcounter.h"
#include "driver_superc.h"
uint8_t Unlimit_Flag = 0;
void bsp_can_init(void)
{
    CAN_FilterTypeDef sFilterConfig = {
            .FilterBank = 0,
            .FilterMode = CAN_FILTERMODE_IDMASK,
            .FilterScale = CAN_FILTERSCALE_16BIT,
            .FilterIdHigh = 0x0000,
            .FilterIdLow = 0x0000,
            .FilterMaskIdHigh = 0x0000,
            .FilterMaskIdLow = 0x0000,
            .FilterFIFOAssignment = CAN_RX_FIFO0,
            .FilterActivation = ENABLE,
            .SlaveStartFilterBank = 14
    };
    HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig);
    HAL_CAN_Start(&hcan1);
    HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);

    //sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO1,
    sFilterConfig.FilterBank = 14;
    HAL_CAN_ConfigFilter(&hcan2, &sFilterConfig);
    HAL_CAN_Start(&hcan2);
    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);//这里原来是fifo1
}


GimbalStatusSturct gimbalStatus;

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
{
    CAN_RxHeaderTypeDef CAN1RxHeader, CAN2RxHeader;
    uint8_t CAN1RxBuffer[8], CAN2RxBuffer[8];

    uint8_t i;
    if (hcan->Instance == CAN1)
    {
        HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &CAN1RxHeader, CAN1RxBuffer);
        switch (CAN1RxHeader.StdId)
        {
            case CAN1_FILTER_ID_CHASSIS_MOTOR1:
                for (i = 0; i < 8; i++)
                    ChassisMotor[1].ReceiveMessege[i] = CAN1RxBuffer[i];
                MotorDataUpdate(&ChassisMotor[1]);
                LostCounterFeed(CHASSIS_MOTOR_0);
                break;
            case CAN1_FILTER_ID_CHASSIS_MOTOR2:
                for (i = 0; i < 8; i++)
                    ChassisMotor[2].ReceiveMessege[i] = CAN1RxBuffer[i];
                MotorDataUpdate(&ChassisMotor[2]);
                LostCounterFeed(CHASSIS_MOTOR_1);
                break;
            case CAN1_FILTER_ID_CHASSIS_MOTOR3:
                for (i = 0; i < 8; i++)
                    ChassisMotor[3].ReceiveMessege[i] = CAN1RxBuffer[i];
                MotorDataUpdate(&ChassisMotor[3]);
                LostCounterFeed(CHASSIS_MOTOR_2);
                break;
            case CAN1_FILTER_ID_CHASSIS_MOTOR4:
                for (i = 0; i < 8; i++)
                    ChassisMotor[4].ReceiveMessege[i] = CAN1RxBuffer[i];
                MotorDataUpdate(&ChassisMotor[4]);
                LostCounterFeed(CHASSIS_MOTOR_3);
                break;
            //因为can线堵塞的问题，把电容从can2上移动到can1上
            case RECEIVE_FROM_SUPERC_FILTER_ID:
                SC_recv_message(CAN1RxBuffer);
        }
    }

    if (hcan->Instance == CAN2)
    {
        HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &CAN2RxHeader, CAN2RxBuffer);
        switch (CAN2RxHeader.StdId)
        {
            case GIMBAL_FILTER_ID:
                GimbalDataUpdate(CAN2RxBuffer);
                ChassisControlDataUpdate(&gimbalControlData);
                LostCounterFeed(REMOTE_LOST_COUNT);
                break;
            case GIMBAL_FILTER_ID2:
                /**
				 * [0]		[1]			[2]		[3]								[4]		[5]		[6]		[7]
				 * pitchAngle(2Bytes)	error	fricRank和自瞄状态	           云台yaw的setloction(4Bytes)
                 *
                 * uint8_t err      6、7位为旧英雄所用
                 * 7            6           5           4           3           2           1           0
                 * vision       pitch       L_fri       R_fri       H_fir       Feed       small_p     glass
                 *  视觉     云台pitch       左摩擦轮      右摩擦轮     上摩擦轮      拨弹       小云台pitch   瞄准镜
				 * **/
                for (i = 0; i < 8; i++)
                    gimbalStatus.data[i] = CAN2RxBuffer[i];

                if (!(gimbalStatus.chassisData.errorStatus & 0x01))
                    LostCounterFeed(SCOPE_LOST_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 1) & 0x01))
                    LostCounterFeed(SMALL_PITCH_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 2) & 0x01))
                    LostCounterFeed(FEEDMOTOR_LOST_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 3) & 0x01))
                    LostCounterFeed(H_FRICTION_LOST_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 4) & 0x01))
                    LostCounterFeed(R_FRICTION_LOST_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 5) & 0x01))
                    LostCounterFeed(L_FRICTION_LOST_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 6) & 0x01))
                    LostCounterFeed(GIMBAL_MOTOR_PITCH);
                if (!((gimbalStatus.chassisData.errorStatus >> 7) & 0x01))
                    LostCounterFeed(VISION_LOST_COUNT);
                if((gimbalStatus.chassisData.heroStatus >> 4) & 0x01)
                    Unlimit_Flag = 1;
                else
                    Unlimit_Flag = 0;

#if NO_HERO == 0
                if (!((gimbalStatus.chassisData.errorStatus >> 6) & 0x01))
                    LostCounterFeed(SMALL_PITCH_COUNT);
                if (!((gimbalStatus.chassisData.errorStatus >> 7) & 0x01))
                    LostCounterFeed(SCOPE_LOST_COUNT);
#endif
                break;
            case CAN2_FILTER_ID_YAW_MOTOR:
                LostCounterFeed(GIMBAL_MOTOR_YAW);
                for (i = 0; i < 8; i++)
                    yawMotor.ReceiveMessege[i] = CAN2RxBuffer[i];
                Motor4310FeedbackFrameDataUpdate(&yawMotor);
                FollowControlDataUpdate(&yawMotor);
                LostCounterFeed(GIMBAL_MOTOR_YAW);
        }
    }

}